﻿using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using ShevaEngine.Core.Foundation;

namespace ShevaEngine.Core.Modules.ArtificialIntelligence.Pathfinding
{
    /// <summary>
    /// Navigation graph class.
    /// </summary>
    public class NavigationGraph
    {
        /// <summary>Navigation mesh nodes.</summary>
        public List<NavigationGraphNode> Nodes { get; private set; }
        
        /// <summary>
        /// Constructor.
        /// </summary>
        public NavigationGraph()
        {
            this.Nodes = new List<NavigationGraphNode>();
        }

        /// <summary>
        /// Method adds node.
        /// </summary>
        /// <param name="node"></param>
        public void AddNode(NavigationGraphNode node)
        {
            this.Nodes.Add(node);
        }

        /// <summary>
        /// Method adds nodes.
        /// </summary>
        /// <param name="nodes"></param>
        public void AddNodes(IEnumerable<NavigationGraphNode> nodes)
        {
            this.Nodes.AddRange(nodes);
        }

        /// <summary>
        /// Method finds way between start and end.
        /// </summary>
        /// <param name="start"></param>
        /// <param name="end"></param>
        /// <param name="path"></param>
        /// <returns></returns>
        public bool FindPath(Vector3 start, Vector3 end, ref NavigationPath path, Heuristic heuristic = null)
        {
            path.Clear();

            if (heuristic == null)
                heuristic = new DistanceHeuristic();

            NavigationGraphNode startNode = this.GetNodeWithLocation(start);
            NavigationGraphNode endNode = this.GetNodeWithLocation(end);

            if (startNode != null && endNode != null)
            {
                if (startNode == endNode)
                    return true;

                heuristic.EndNode = endNode;

                List<NavigationGraphNodeRecord> openNodes = new List<NavigationGraphNodeRecord>();
                openNodes.Add(new NavigationGraphNodeRecord(startNode));
                List<NavigationGraphNodeRecord> closedNodes = new List<NavigationGraphNodeRecord>();

                NavigationGraphNodeRecord currentNode = openNodes.OrderBy(node => node.CostSoFar).First();

                while (openNodes.Count > 0)
                {
                    currentNode = openNodes.OrderBy(node => node.EstimatedTotalCost).First();

                    if (currentNode.Node == endNode)
                        break;

                    foreach (NavigationGraphConnection connection in currentNode.Node.Connections)
                    {
                        NavigationGraphNodeRecord endConnectionNode = new NavigationGraphNodeRecord(
                            connection.Node1, connection, currentNode.CostSoFar + connection.Cost);

                        if (currentNode.Node == connection.Node1)
                            endConnectionNode.Node = connection.Node2;

                        if (closedNodes.Contains(endConnectionNode, NavigationGraphNodeRecordEqualityComparer.Instance))
                        {
                            NavigationGraphNodeRecord temp = closedNodes.First(item => item.Node == endConnectionNode.Node);
                                                                
                            if (temp.CostSoFar <= endConnectionNode.CostSoFar)
                                continue;

                            closedNodes.Remove(endConnectionNode);
                        }
                        else if (openNodes.Contains(endConnectionNode, NavigationGraphNodeRecordEqualityComparer.Instance))
                        {
                            NavigationGraphNodeRecord temp = openNodes.First(item => item.Node == endConnectionNode.Node);

                            if (temp.CostSoFar <= endConnectionNode.CostSoFar)
                                continue;
                        }

                        endConnectionNode.EstimatedTotalCost = heuristic.Estimate(endConnectionNode.Node);

                        openNodes.Add(endConnectionNode);
                    }                    
                    
                    openNodes.Remove(currentNode);
                    closedNodes.Add(currentNode);
                }

                if (currentNode.Node != endNode)
                    return false;

                while (currentNode.Node != startNode)
                {
                    path.Push(currentNode.Connection);

                    NavigationGraphNode temp = currentNode.Connection.Node1;

                    if (currentNode.Node == temp)
                        temp = currentNode.Connection.Node2;

                    currentNode = closedNodes.First(node => node.Node == temp);
                }

                return true;
            }

            return false;
        }

        /// <summary>
        /// Method returns NavigationNode with location.
        /// </summary>
        /// <param name="vertex"></param>
        /// <returns></returns>
        public NavigationGraphNode GetNodeWithLocation(Vector3 vertex)
        {
            foreach (NavigationGraphNode node in this.Nodes)
                if (node.ContainsLocation(vertex))
                    return node;
            
            return null;
        }

        /// <summary>
        /// Method creates navigation graph from regular grid.
        /// </summary>
        /// <param name="cells"></param>
        /// <param name="gridDim"></param>
        /// <returns></returns>
        public static NavigationGraph CreateFromRegularGrid(IEnumerable<Pair<Int32, Int32>> cells, float gridDim, Vector3 offset)
        {
            NavigationGraph outputGraph = new NavigationGraph();

            SortedList<Pair<Int32, Int32>, NavigationGraphNode> nodes =
                new SortedList<Pair<int, int>, NavigationGraphNode>(new PairComparer<Int32, Int32>());

            foreach (Pair<Int32, Int32> cell in cells)
            {
                Quad2D newQuad = new Quad2D();
                newQuad.Center = new Vector3(cell.First * gridDim + offset.X, offset.Y, cell.Second * gridDim + offset.Z);
                newQuad.Dimension = new Vector2(gridDim, gridDim);

                nodes.Add(cell, new NavigationGraphNodeQuad2D(newQuad));
            }

            foreach (KeyValuePair<Pair<Int32, Int32>, NavigationGraphNode> node in nodes)
            {
                Pair<Int32, Int32> left = new Pair<int, int>(node.Key.First - 1, node.Key.Second);
                Pair<Int32, Int32> down = new Pair<int, int>(node.Key.First, node.Key.Second - 1);
                Pair<Int32, Int32> leftdown = new Pair<int, int>(node.Key.First - 1, node.Key.Second - 1);
                Pair<Int32, Int32> rightdown = new Pair<int, int>(node.Key.First + 1, node.Key.Second - 1);
                Pair<Int32, Int32> right = new Pair<int, int>(node.Key.First + 1, node.Key.Second);

                if (nodes.ContainsKey(left))
                {
                    NavigationGraphConnection newConnection = new NavigationGraphConnection(
                        (node.Value as NavigationGraphNodeQuad2D).Quad.Center -
                            new Vector3((node.Value as NavigationGraphNodeQuad2D).Quad.Dimension.X  / 2, 0,
                                (node.Value as NavigationGraphNodeQuad2D).Quad.Dimension.Y / 2),
                        (node.Value as NavigationGraphNodeQuad2D).Quad.Center -
                            new Vector3((node.Value as NavigationGraphNodeQuad2D).Quad.Dimension.X  / 2, 0,
                                -(node.Value as NavigationGraphNodeQuad2D).Quad.Dimension.Y / 2),
                        nodes[node.Key], nodes[left], 1.0f);

                    nodes[node.Key].Connections.Add(newConnection);
                    nodes[left].Connections.Add(newConnection);
                }

                if (nodes.ContainsKey(down))
                {
                    NavigationGraphConnection newConnection = new NavigationGraphConnection(
                        (node.Value as NavigationGraphNodeQuad2D).Quad.Center -
                            new Vector3((node.Value as NavigationGraphNodeQuad2D).Quad.Dimension.X / 2, 0,
                                (node.Value as NavigationGraphNodeQuad2D).Quad.Dimension.Y / 2),
                        (node.Value as NavigationGraphNodeQuad2D).Quad.Center -
                            new Vector3(-(node.Value as NavigationGraphNodeQuad2D).Quad.Dimension.X / 2, 0,
                                (node.Value as NavigationGraphNodeQuad2D).Quad.Dimension.Y / 2),
                        nodes[node.Key], nodes[down], 1.0f);

                    nodes[node.Key].Connections.Add(newConnection);
                    nodes[down].Connections.Add(newConnection);
                }

                if (nodes.ContainsKey(down) && nodes.ContainsKey(left) && nodes.ContainsKey(leftdown))
                {
                    NavigationGraphConnection newConnection = new NavigationGraphConnection(
                        (nodes[left] as NavigationGraphNodeQuad2D).Quad.Center,
                        (nodes[down] as NavigationGraphNodeQuad2D).Quad.Center,
                        nodes[node.Key], nodes[leftdown], 1.4f);

                    nodes[node.Key].Connections.Add(newConnection);
                    nodes[leftdown].Connections.Add(newConnection);
                }

                if (nodes.ContainsKey(down) && nodes.ContainsKey(right) && nodes.ContainsKey(rightdown))
                {
                    NavigationGraphConnection newConnection = new NavigationGraphConnection(
                        (nodes[right] as NavigationGraphNodeQuad2D).Quad.Center,
                        (nodes[down] as NavigationGraphNodeQuad2D).Quad.Center,
                        nodes[node.Key], nodes[rightdown], 1.4f);

                    nodes[node.Key].Connections.Add(newConnection);
                    nodes[rightdown].Connections.Add(newConnection);
                }
            }

            outputGraph.AddNodes(nodes.Values);

            return outputGraph;
        }
    }
}
